#!/usr/bin/env python
#coding=utf-8
import rospy
import time
import os 
import sys
import numpy as np
import cv2 
from cv_bridge import CvBridge
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist,PoseWithCovarianceStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from sensor_msgs.msg import Image as ros_img,LaserScan
from std_msgs.msg import Int32,String
import pyzbar.pyzbar as pyzbar
from math import pi
from aikit_arm.srv import claw
from ros_openvino.msg import Objects
from aip import AipSpeech 

class robot():
    def __init__(self):
        rospy.init_node('aikit_robot', anonymous=True)
        self.move_start()
        self.nlp_start()
        self.nav_start()
        self.arm_start()
        self.camera_start()
        self.object_detection()
        self.tts_start()
        self.lidar_start()
        self.setpose_pub = rospy.Publisher("initialpose",PoseWithCovarianceStamped,latch=True, queue_size=1)

    def move_start(self):
        self.move_pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
        self.rate = rospy.Rate(10) # 10hz
        self.cmd_vel_data=Twist()

    def nlp_start(self):
        self.asr_result=""
        self.tuling_result=""
        self.tts_pub=rospy.Publisher("tts_topic",String,queue_size=1)
        self.asr_pub=rospy.Publisher("asr_topic",Int32,queue_size=1)
        self.tuling_input_pub=rospy.Publisher("tuling_input",String,queue_size=1)
        self.asr_start=rospy.Subscriber("nlu_topic",String,self.asr_callback,queue_size=1)
        self.tuling_out=rospy.Subscriber("tuling_output",String,self.tuling_callback,queue_size=1)
    
    def nav_start(self):
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) #实例化action服务
        rospy.loginfo('等待move_base action服务器连接...')
        self.move_base.wait_for_server(rospy.Duration(30)) #等待连接服务
        rospy.loginfo('已连接导航服务')  #提示连接成功

    def arm_start(self):
        rospy.wait_for_service("claw")
        rospy.loginfo('已连接机械臂控制服务')
        self.claw = rospy.ServiceProxy("claw", claw)
    
    def camera_start(self):
        self.cvb=CvBridge()
        self.camera_image=np.zeros((640,480,3),np.uint8)
        self.camera_sub=rospy.Subscriber("camera/rgb/image_raw", ros_img, self.read_img)
    def lidar_start(self):
        self.lidar_ranges=[]
        self.lidar_sub=rospy.Subscriber("scan",LaserScan,self.lidar_callback)
    def object_detection(self):
        self.objects_data=[]
        self.object_result=""
        self.object_sub=rospy.Subscriber("object_detection/results",Objects,self.object_callback)

    def read_img(self,image):   
        image=self.cvb.imgmsg_to_cv2(image,desired_encoding="passthrough")
        self.camera_image=cv2.cvtColor(image,cv2.COLOR_RGB2BGR)
    
    def asr_callback(self,data):
        self.asr_result=data.data
    
    def tuling_callback(self,data):
        self.tuling_result=data.data
    
    def object_callback(self,data):
        self.objects_data=data.objects
        # if len(objects_data)==1:
        #     self.object_result=self.objects_data[0].label
        # if len(objects_data)==2:
        #     if objects_data[0].y>objects_data[1].y:
        #         self.object_result=self.objects_data[0].label
        #     else:
        #         self.object_result=self.objects_data[1].label
        # if len(objects_data)>2:
        #     self.object_result=self.objects_data[0].label
        
    def lidar_callback(self,data):
        self.lidar_ranges=data.ranges
    #功能函数
    def turn(self,turn_speed=0,times=0):   
        t=time.time()
        self.cmd_vel_data.linear.x=0
        self.cmd_vel_data.angular.z=turn_speed
        while not rospy.is_shutdown() and (time.time()-t)<times:
            self.move_pub.publish(self.cmd_vel_data)
            self.rate.sleep()
        self.move_pub.publish(Twist())
    def go(self,go_speed=0,times=0):
        t=time.time()
        self.cmd_vel_data.linear.x=go_speed
        self.cmd_vel_data.angular.z=0
        while not rospy.is_shutdown() and (time.time()-t)<times:
            self.move_pub.publish(self.cmd_vel_data)
            self.rate.sleep()
        self.move_pub.publish(Twist())
    def set_speed(self,lx=0.0,az=0.0):
        self.cmd_vel_data.linear.x=lx
        self.cmd_vel_data.angular.z=az
        self.move_pub.publish(self.cmd_vel_data)
        
    def nav_to(self,x,y,d):#导航函数，添加坐标点,输入x（前）坐标，y（左）坐标，th（平面朝向-360～360度）
        goal = MoveBaseGoal()  #实例化目标点消息
        goal.target_pose.header.frame_id='map'  #设置目标tf
        goal.target_pose.header.stamp = rospy.Time.now()  #获取系统时间戳
        goal.target_pose.pose.position.x=float(x)  #设置x坐标
        goal.target_pose.pose.position.y=float(y)  #设置y坐标
        quaternion=quaternion_from_euler(0.0,0.0,float(d)/180.0*pi) #根据传入欧拉角d转四元数
        goal.target_pose.pose.orientation.x=quaternion[0] #四元数x赋值
        goal.target_pose.pose.orientation.y=quaternion[1] #四元数y赋值
        goal.target_pose.pose.orientation.z=quaternion[2] #四元数z赋值
        goal.target_pose.pose.orientation.w=quaternion[3] #四元数w赋值
        rospy.loginfo('尝试导航去坐标点:('+str(x)+","+str(y)+','+str(d)+')')  #提示连接成功
        self.move_base.send_goal(goal) #发送导航目标
        finished_within_time = self.move_base.wait_for_result(rospy.Duration(35))  #等待10秒，获取导航结果
        if not finished_within_time: #如果导航失败
            self.move_base.cancel_goal() #取消导航
            rospy.loginfo('时间超时，导航任务取消。') #打印取消导航
            return 0
        state = self.move_base.get_state() #获取导航状态
        if state == GoalStatus.SUCCEEDED: #如果导航状态为SUCCEED
            rospy.loginfo('导航成功！') #提示导航成功
            return 1
    def cancel_goal(self):
        self.move_base.cancel_goal() 

    def set_pose(self,x=0.0,y=0.0,a=0.0):
        rospy.loginfo("start set pose...")
        p = PoseWithCovarianceStamped()
        p.header.frame_id = "map"
        p.pose.pose.position.x = float(x)
        p.pose.pose.position.y = float(y)
        (p.pose.pose.orientation.x,p.pose.pose.orientation.y,p.pose.pose.orientation.z,p.pose.pose.orientation.w)=quaternion_from_euler(0.0,0.0,float(a)/180.0*pi)
        p.pose.covariance[6 * 0 + 0] = 0.5 * 0.5
        p.pose.covariance[6 * 1 + 1] = 0.5 * 0.5
        p.pose.covariance[6 * 5 + 5] = pi / 12.0 * pi / 12.0
        p.header.stamp = rospy.Time.now()
        self.setpose_pub.publish(p)
        rospy.sleep(2)
        rospy.loginfo("end set pose...")

    def speak(self,words):
        self.tts_pub.publish(words)
        rospy.loginfo('开始语音合成：')
        rospy.loginfo(words)

    def listen(self,times=10):
        rospy.loginfo("请开始说话...")
        self.asr_result=""
        self.asr_pub.publish(1)
        t=time.time()
        while not rospy.is_shutdown() and (time.time()-t)<times:
            if len(self.asr_result)>0:
                rospy.loginfo("语音识别结果为：")
                rospy.loginfo(self.asr_result)
                return self.asr_result

    def tuling(self,words,times=10):
        rospy.loginfo("向图灵机器人发送问题...")
        self.tuling_result=''
        self.tuling_input_pub.publish(words)
        t=time.time()
        while not rospy.is_shutdown() and (time.time()-t)<times:
            if len(self.tuling_result)>0:
                rospy.loginfo("从图灵获得答案：")
                rospy.loginfo(self.tuling_result)
                return self.tuling_result

    def set_claw(self,pos):
        self.claw(pos)
    def read_camera(self):
        return self.camera_image
    def object_data(self):
        return self.object_data
    def object_label(self):
        object_result=""
        rospy.loginfo("开始识别......")
        t=time.time()
        count=0
        last_result=""
        while not rospy.is_shutdown() and (time.time()-t)<10:
            objects_data=self.objects_data
            if objects_data:
                object_data= objects_data[0]
                p=object_data.confidence
                object_name=object_data.label
                if p>=0.4 and object_name==last_result:                 
					count+=1          
                else:
                    count=0
                last_result=object_name
                if count>=10:  
                    return object_name               


    def tts_start(self):
        self.APP_ID = '11571096'  
        self.API_KEY = 'dD3pVVfIpBciC9IG7W3VbqNr'
        self.SECRET_KEY = 'dBr9fu5lVrjkmOQl1VpOKDlXiHGqKaTW'
        self.client = AipSpeech(self.APP_ID,self.API_KEY,self.SECRET_KEY) 
    
    def tts(self,words,language="zh",speed=5,pit=5,person=0,volume=10,):
	    result= self.client.synthesis(words, language, 1, {
	        'spd': speed,   #语速 0最小 9最大
	        'pit': pit,    #音调 0最小 9最大   
	        'vol': volume,  #音量 0最小 15最大
	        'per': person   # 发音人 0：女声 1：男声 3:度逍遥 4：度丫丫
	        })

	    if not isinstance(result, dict):
		    file_name = "/home/robot/Music/ros_voice_system/"+words+".mp3"     
		    with open(file_name, 'wb') as f:  
			    f.write(result) 
		    os.system('play '+file_name)
    def play_sound(self,words):
        rospy.loginfo("尝试播放本地音频")
        file_path="/home/robot/Music/ros_voice_system/"+words+".mp3"
        result=os.system("play "+file_path)   
        if result==512:
            rospy.loginfo("本地播放失败，尝试在线合成，请确认网络")
            self.tts(words)
    def lidar_range(self,direction=180):
        while True:
            distance_list=self.lidar_ranges
            if len(distance_list)>0:
                distance=distance_list[direction]
                if distance != float("inf"):
                    return distance
        


            



        

    







